#include <Log.h>
#include <Device.h>
#include <Hw/Xmc.h>
#include <Hw/Memory.h>

#include "mcbl.h"
#include <ti/csl/cslr.h>
#include <ti/csl/csl_bootcfgAux.h>

#ifndef BUILD_BOOTLOADER
#include <stdio.h>
#define msg Log_msg
#else
char __section(".stack") _stack[8];
const char __section(".ver") DATE[] = __DATE__;
const char __section(".ver") TIME[] = __TIME__;


#ifndef __TI_EABI__
__asm("\t.global __STACK_END");
__asm("\t.global _main");
#else
__asm("\t.global main");
__asm("\t.global __TI_STACK_END");
__asm("\t.global __TI_STATIC_BASE");
#endif
__asm("\t.global _c_int00");
/*****************************************************************************/
/* _c_int00(): boot entry after power on                                   */
/*****************************************************************************/
extern far void _c_int00();
__asm("\t.sect \".boot\"");
__asm("_c_int00:");
#ifndef __TI_EABI__
   __asm("\t   MVKL\t\t   _main,B0");
   __asm("\t   MVKH\t\t   _main,B0");
#else
   __asm("\t   MVKL\t\t   main,B0");
   __asm("\t   MVKH\t\t   main,B0");
#endif
   __asm("\t   B          B0");

#ifndef __TI_EABI__
   __asm("\t   MVKL\t\t   __STACK_END - 4, SP");
   __asm("\t   MVKH\t\t   __STACK_END - 4, SP");
#else
   __asm("\t   MVKL\t\t   __TI_STACK_END - 4, SP");
   __asm("\t   MVKH\t\t   __TI_STACK_END - 4, SP");
#endif
   __asm("\t   AND\t\t   ~7,SP,SP");

#ifndef __TI_EABI__
   __asm("\t   MVKL\t\t   $bss,DP");
   __asm("\t   MVKH\t\t   $bss,DP");
#else
   __asm("\t   MVKL\t\t   __TI_STATIC_BASE,DP");
   __asm("\t   MVKH\t\t   __TI_STATIC_BASE,DP");
#endif

static inline
void
memcpy4(
        void* des,
        void* src,
        int32_t n/*bytes*/
){
    Uint32* d = des;
    Uint32* s = src;
    //Qbytess
    for (n >>= 2; n > 0; n--)
        *d++ = *s++;
}


#include <c6x.h>

void __init main (void) {
    TSCL = 0;

    CorePac core = (CorePac) DNUM;
    if (COREPAC_MASTER == core) {
        memcpy4((void*)LOADER_RUN, (void*)LOADER_LOAD, LOADER_LOAD_SZ);
        Cache_wbInvAll();
    }
    Hw_socSetup(core);

    load((McblImgDesc*) MCBL_TLB_BASE, core);

    //dummy call to link DATE & TIME;
    TSCL = DATE[0];
    TSCL = TIME[0];
}

static inline void msg(const char* fmt, ...) { return; }
#endif


#define IPC_BOOT 1

Entry __section(".x") xentry;// = (Entry)INVALID;

static inline
Entry
__section(".loader")
loadCoreBtbl            //load executable image
(   Btbl*   ibli         //Btbl entity's position
   ,CorePac core        //to core's memory view.
){
    msg("load core-%d from %x\n", core, ibli);

    if (!CSL_FEXT((Uint32)ibli, MCBL_ENTRY_VALID)) {
        msg("\tinvalid entry, invalid!\n");
        return (Entry)INVALID; }

    if (INVALID == (int32_t)ibli) {
        msg("\tinvalid entry!\n");
        return (Entry)INVALID; }

    Btbl* ibl = (Btbl*)((Uint32)ibli & MCBL_ENTRY_VAL_MASK);

    Btx* btx = (Btx*)&ibl->bt0.head.length;
    int32_t size = btx->head.length;
    while ((NONE != size) && (INVALID != size)) {
        void*   to = (void*)btx->head.to;
        void*   from = (void*)&btx->payload;

        msg("\tsection %x + %x -> %x\n", from, size, to);
        memcpy4(to, from, size);

        btx = (Btx*) &btx->payload[size];
        size = btx->head.length;
    }

    msg("\tentry@%x", ibl->bt0.head.entry);
    return (Entry)ibl->bt0.head.entry;
}

void __section(".loader")
load(
    McblImgDesc* descriptor,
    CorePac      core
){
    short coreCnt = descriptor->CORES;
    Btbl* ibl = descriptor->BTBLT[coreCnt];

    Entry entry;
    if (core == COREPAC0) {
        msg("\tmaster core, load base image if has.\n");
        xentry = loadCoreBtbl(ibl, (CorePac)coreCnt); }
    ibl = descriptor->BTBLT[core];
    entry = loadCoreBtbl(ibl, (CorePac)core);

    Entry ientry;
    if (entry != (Entry)INVALID) {
        ientry = entry; }
    else if ((xentry != (Entry)INVALID) && (CSL_FEXT((Uint32)ibl, MCBL_ENTRY_VALID))) {
        ientry = xentry; }
    else {
        ientry = (Entry)INVALID; }
    msg("\tentry@%x", ientry);

    if (core < coreCnt) {
        msg("\twake up core next.\n");
        core++;
        CSL_BootCfgUnlockKicker();
        *(Uint32*) BOOT_MAGIC(core) = (Uint32)_c_int00;
        CSL_BootCfgGenerateInterDSPInterrupt(core, 0);
        CSL_BootCfgLockKicker();
    }

    if (ientry != (Entry)INVALID) {
        msg("\tstart...\n");
        Cache_wbInvAll();
        Xmc_setup((CorePac) DNUM, XMC_ROBUST);
        ((Entry)ientry)();
    }

    msg("\tinvalid entry, no load, hung here.\n");
    do {;} while(true);
}
